#!/usr/bin/env python

import sys, rospy, cv2, time
import numpy as np
from lsr import listener
from geometry_msgs.msg import Twist
from Detection import Detection
	

def registeration():
	detection = Detection()
	lateral_error = 1
	longi_error = 1
	listen = listener()
	order = 3
	
	while True:
		order = listen.meet()
		rospy.sleep(1)
		if order == 4:
			break
		x, y, z = detection.get_position()
		print(x, y, z)

		bound = 3
		if x < 320 - 3* bound:
			move_cmd.linear.x = 0.05
			move_cmd.angular.z = 0.2
			lateral_error = 1
		elif x > 320 + bound:
			move_cmd.linear.x = 0.05
			move_cmd.angular.z = -0.2
			lateral_error = 1
		else:
			move_cmd.linear.x = 0.0
			move_cmd.angular.z = 0.0
			lateral_error = 0

		cmd_vel.publish(move_cmd)
		rospy.sleep(0.1)

		if z > 600:
			move_cmd.linear.x = 0.1
			move_cmd.angular.z = 0
			longi_error = 1
		elif z < 400:
			move_cmd.linear.x = -0.1
			move_cmd.angular.z = 0
			longi_error = 1
		else:
			move_cmd.linear.x = 0
			move_cmd.angular.z = 0
			longi_error = 0

		cmd_vel.publish(move_cmd)
		rospy.sleep(0.1)


		



		# if lateral_error == 0:
		# 	break
	print("left-right is ok")
	while 0:
		x, y, z = detection.get_position()
		print(x, y, z)

		#if z > 1000:
		#	lateral_error = 1
		#	while True:
		#		bound = 5
		#		if x < 320 - bound:
		#			move_cmd.linear.x = 0.01
		#			move_cmd.angular.z = 0.2
		#			lateral_error = 1	
		#		elif x > 320 + bound:
		#			move_cmd.linear.x = 0.01
		#			move_cmd.angular.z = -0.2
		#			lateral_error = 1
		#		else:
		#			move_cmd.linear.x = 0.0
		#			move_cmd.angular.z = 0.0
		#			lateral_error = 0

		#		cmd_vel.publish(move_cmd)
		#		rospy.sleep(0.1)

#				if lateral_error == 0:
#					break
		if z > 250:
			move_cmd.linear.x = 0.05
			move_cmd.angular.z = 0
			longi_error = 1
		elif z < 240:
			move_cmd.linear.x = -0.05
			move_cmd.angular.z = 0
			longi_error = 1
		else:
			move_cmd.linear.x = 0
			move_cmd.angular.z = 0
			longi_error = 0

		cmd_vel.publish(move_cmd)
		rospy.sleep(0.1)

		if longi_error == 0:
			break
			
			
	lateral_error = 1
	while 0:
		x, y, z = detection.get_position()
		print(x, y, z)

		bound = 5
		if x < 320 + 3*bound:
			move_cmd.linear.x = 0.01
			move_cmd.angular.z = 0.2
			lateral_error = 1
		elif x > 320 + 5*bound:
			move_cmd.linear.x = 0.01
			move_cmd.angular.z = -0.2
			lateral_error = 1
		else:
			move_cmd.linear.x = 0.0
			move_cmd.angular.z = 0.0
			lateral_error = 0

		cmd_vel.publish(move_cmd)
		rospy.sleep(0.1)

		if lateral_error == 0:
			break
	print("left-right is ok")
		
	move_cmd.linear.x = 0.05
	move_cmd.angular.z = 0.0

	for i in range(19):
		cmd_vel.publish(move_cmd)
		rospy.sleep(0.1)

	move_cmd.linear.x = 0.0
	move_cmd.angular.z = 0.0
	cmd_vel.publish(move_cmd)
	rospy.sleep(1)
	

			

if __name__ == '__main__':
	rospy.init_node('photo', anonymous=False)
	cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
	move_cmd = Twist()
	listen = listener()
	order = 2
	while True:
		order = listen.meet()
		rospy.sleep(1)
		if order == 3:
			break
		else:
			continue
	registeration()
	listen.help()

